from planning.scripts.fingers_action_client import unitParser as finger_pos_unitParser
from planning.scripts.pose_action_client import unitParser as eef_pose_unitParser
from planning.scripts.pose_action_client import getcurrentCartesianCommand
from planning.scripts.joints_action_client import unitParser as joint_unitParser
from planning.scripts.joints_action_client import getcurrentJointCommand
from planning.scripts.robot_control_modules import *

def initial_joint_pose(prefix):
    """
    rostopic echo /j2n6s300_driver/out/joint_command
    joint1: 85.0451889038
    joint2: 187.378189087
    joint3: 95.0190429688
    joint4: -14.20980835
    joint5: 65.8101730347
    joint6: -46.50952148399995
    joint7: 0.0
    """
    positions = [97.924407959, 164.200363159, 56.3846130371, 352.41317749, 39.6821975708, -29.5866718292, 0.0]
    # positions = [78.7904052734, 164.925323486, 95.1837768555, 340.085632324, 94.2306747437, -73.360961914, 0.0]
    try:           
        result = joint_position_client(positions, prefix)
    except rospy.ROSInterruptException:
        print('program interrupted before completion')


def finger_action(position, prefix, unit='percent', relative=False):
    '''
    args:
        position: list, [finger_1,finger_2,finger_3]
    return:
        result
    '''
    finger_turn, _, _ = finger_pos_unitParser(unit, position, relative)
    result = gripper_client(finger_turn, prefix)

    return result

def arm_action(pose, prefix, unit='mq', relative=False):
    '''
    args:
        pose: list, [position, orientation]
    return:
        result
    '''
    getcurrentCartesianCommand(prefix)
    pose_mq, _, _ = eef_pose_unitParser(unit, pose, relative)
    poses = [float(n) for n in pose_mq]
    result = cartesian_pose_client(poses[:3], poses[3:], prefix)

    return result

def arm_take_up_action(height, prefix):
    '''
    args: 
        height: float, height that arm take up
    return:
        result
    '''
    getcurrentCartesianCommand(prefix)
    pose_mq_takeup, _, _ = eef_pose_unitParser('mq', [0,0,height,0,0,0,1], True)
    poses = [float(n) for n in pose_mq_takeup]
    result = cartesian_pose_client(poses[:3], poses[3:], prefix)  

    return result


def moveit_arm_plan(arm_group, pose):
    '''
    args:
        arm_group: moveit_commander.MoveGroupCommander
        pose: PoseStamped
    return:
        result
    '''
    arm_group.set_pose_target(pose)
    plan = arm_group.plan()
    result = arm_group.go(wait=True)

    return result

def get_current_joint(prefix_='j2n6s300_'):
    '''
    args: 
        prefix_: type of arm 
    return:
        joint_degree
    '''
    getcurrentJointCommand(prefix_)
    joint_degree, joint_radian = joint_unitParser('degree', [0]*7, True)
    return joint_degree, joint_radian


def arm_change_joint_action(degree=[90,0,0,0,0,0,0], prefix_='j2n6s300_', unit='degree', relative=True):
    '''
    args: 
        degree: float[7], degrees that rotate the arm's joint
    return:
        result
    '''
    getcurrentJointCommand(prefix_)
    joint_degree, joint_radian = joint_unitParser(unit, degree, relative)
    result = joint_position_client(joint_degree, prefix_)
    return result